/**************************************************************************************************
* Title: SumMatrix_16f.inl
* Author: Gael Huber
* Description: Implements 16-byte aligned matrix functions
**************************************************************************************************/

/**************************************************************************************************
* Default constructor
**************************************************************************************************/
inline Matrix_16f::Matrix_16f(void) {
	v[0] = _mm_setzero_ps();
	v[1] = _mm_setzero_ps();
	v[2] = _mm_setzero_ps();
	v[3] = _mm_setzero_ps();
}

/**************************************************************************************************
* Constructor from vector array
**************************************************************************************************/
inline Matrix_16f::Matrix_16f(const __m128* f) {
	v[0] = f[0];
	v[1] = f[1];
	v[2] = f[2];
	v[3] = f[3];
}

/**************************************************************************************************
* Constructor from float array
**************************************************************************************************/
inline Matrix_16f::Matrix_16f(const float* f) {
	v[0] = _mm_load_ps(f);
	v[1] = _mm_load_ps(f + 4);
	v[2] = _mm_load_ps(f + 8);
	v[3] = _mm_load_ps(f + 12);
}

/**************************************************************************************************
* Constructor specifying vector values
**************************************************************************************************/
inline Matrix_16f::Matrix_16f(const __m128& r0, const __m128& r1, const __m128& r2, const __m128& r3) {
	v[0] = r0;
	v[1] = r1;
	v[2] = r2;
	v[3] = r3;
}

/**************************************************************************************************
* Constructor specifying values
**************************************************************************************************/
inline Matrix_16f::Matrix_16f(	
	float _11, float _12, float _13, float _14,
	float _21, float _22, float _23, float _24,
	float _31, float _32, float _33, float _34,
	float _41, float _42, float _43, float _44)
	:	_11(_11), _12(_12), _13(_13), _14(_14),
		_21(_21), _22(_22), _23(_23), _24(_24),
		_31(_31), _32(_32), _33(_33), _34(_34),
		_41(_41), _42(_42), _43(_43), _44(_44)
{ }

/**************************************************************************************************
* Copy constructor
**************************************************************************************************/
inline Matrix_16f::Matrix_16f(const Matrix_16f& rhs) {
	v[0] = rhs.v[0];
	v[1] = rhs.v[1];
	v[2] = rhs.v[2];
	v[3] = rhs.v[3];
}

/**************************************************************************************************
* Destructor
**************************************************************************************************/
inline Matrix_16f::~Matrix_16f(void)
{ }

/**************************************************************************************************
* Non-const data accessor
**************************************************************************************************/
inline float& Matrix_16f::operator() (unsigned int row, unsigned int col) {
	return m[row][col];
}

/**************************************************************************************************
* Const data accessor
**************************************************************************************************/
inline float Matrix_16f::operator() (unsigned int row, unsigned int col) const {
	return m[row][col];
}

/**************************************************************************************************
* Assignment operator
**************************************************************************************************/
inline Matrix_16f& Matrix_16f::operator=(const Matrix_16f& rhs) {
	if(this == &rhs)
		return *this;
	
	v[0] = rhs.v[0];
	v[1] = rhs.v[1];
	v[2] = rhs.v[2];
	v[3] = rhs.v[3];
	return *this;
}

/**************************************************************************************************
* Add a matrix and return the result
**************************************************************************************************/
inline Matrix_16f& Matrix_16f::operator+=(const Matrix_16f& rhs) {
	v[0] = _mm_add_ps(v[0], rhs.v[0]);
	v[1] = _mm_add_ps(v[1], rhs.v[1]);
	v[2] = _mm_add_ps(v[2], rhs.v[2]);
	v[3] = _mm_add_ps(v[3], rhs.v[3]);
	return *this;
}

/**************************************************************************************************
* Subtract a matrix and return the result
**************************************************************************************************/
inline Matrix_16f& Matrix_16f::operator-=(const Matrix_16f& rhs) {
	v[0] = _mm_sub_ps(v[0], rhs.v[0]);
	v[1] = _mm_sub_ps(v[1], rhs.v[1]);
	v[2] = _mm_sub_ps(v[2], rhs.v[2]);
	v[3] = _mm_sub_ps(v[3], rhs.v[3]);
	return *this;
}

/**************************************************************************************************
* Multiply a matrix by a scalar and return the result
**************************************************************************************************/
inline Matrix_16f& Matrix_16f::operator*=(float rhs) {
	__m128 f(_mm_load1_ps(&rhs));
	v[0] = _mm_mul_ps(v[0], f);
	v[1] = _mm_mul_ps(v[1], f);
	v[2] = _mm_mul_ps(v[2], f);
	v[3] = _mm_mul_ps(v[3], f);
	return *this;
}

/**************************************************************************************************
* Multiplies the matrix by a scalar loaded into a vector
**************************************************************************************************/
inline Matrix_16f& Matrix_16f::operator*=(const __m128& rhs) {
	v[0] = _mm_mul_ps(v[0], rhs);
	v[1] = _mm_mul_ps(v[1], rhs);
	v[2] = _mm_mul_ps(v[2], rhs);
	v[3] = _mm_mul_ps(v[3], rhs);
	return *this;
}

/**************************************************************************************************
* Divides the matrix by a scalar
**************************************************************************************************/
inline Matrix_16f& Matrix_16f::operator/=(float rhs) {
	__m128 f(_mm_load1_ps(&rhs));
	v[0] = _mm_div_ps(v[0], f);
	v[1] = _mm_div_ps(v[1], f);
	v[2] = _mm_div_ps(v[2], f);
	v[3] = _mm_div_ps(v[3], f);
	return *this;
}

/**************************************************************************************************
* Divides the matrix by a scalar loaded into a vector
**************************************************************************************************/
inline Matrix_16f& Matrix_16f::operator/=(const __m128& rhs) {
	v[0] = _mm_div_ps(v[0], rhs);
	v[1] = _mm_div_ps(v[1], rhs);
	v[2] = _mm_div_ps(v[2], rhs);
	v[3] = _mm_div_ps(v[3], rhs);
	return *this;
}

/**************************************************************************************************
* Returns an unmodified copy of the matrix
**************************************************************************************************/
inline Matrix_16f Matrix_16f::operator+(void) const {
	return *this;
}

/**************************************************************************************************
* Returns a negated copy of the matrix
**************************************************************************************************/
inline Matrix_16f Matrix_16f::operator-(void) const {
	__m128 f(_mm_set1_ps(-1.0f));
	return Matrix_16f(
		_mm_mul_ps(v[0], f),
		_mm_mul_ps(v[1], f),
		_mm_mul_ps(v[2], f),
		_mm_mul_ps(v[3], f));
}

/**************************************************************************************************
* Returns the sum of two matrices
**************************************************************************************************/
inline Matrix_16f Matrix_16f::operator+(const Matrix_16f& rhs) const {
	return Matrix_16f(	_mm_add_ps(v[0], rhs.v[0]),
						_mm_add_ps(v[1], rhs.v[1]),
						_mm_add_ps(v[2], rhs.v[2]),
						_mm_add_ps(v[3], rhs.v[3]));
}

/**************************************************************************************************
* Returns the difference between two matrices
**************************************************************************************************/
inline Matrix_16f Matrix_16f::operator-(const Matrix_16f& rhs) const {
	return Matrix_16f(	_mm_sub_ps(v[0], rhs.v[0]),
						_mm_sub_ps(v[1], rhs.v[1]),
						_mm_sub_ps(v[2], rhs.v[2]),
						_mm_sub_ps(v[3], rhs.v[3]));
}

/**************************************************************************************************
* Returns the matrix multiplied by a scalar
**************************************************************************************************/
inline Matrix_16f Matrix_16f::operator*(float rhs) const {
	__m128 f(_mm_load1_ps(&rhs));
	return Matrix_16f(	_mm_mul_ps(v[0], f),
						_mm_mul_ps(v[1], f),
						_mm_mul_ps(v[2], f),
						_mm_mul_ps(v[3], f));
}

/**************************************************************************************************
* Multiplies the matrix by a vector of scalars
**************************************************************************************************/
inline Matrix_16f Matrix_16f::operator*(const __m128& rhs) const {
	return Matrix_16f(	_mm_mul_ps(v[0], rhs),
						_mm_mul_ps(v[1], rhs),
						_mm_mul_ps(v[2], rhs),
						_mm_mul_ps(v[3], rhs));
}

/**************************************************************************************************
* Divides the matrix by a scalar
**************************************************************************************************/
inline Matrix_16f Matrix_16f::operator/(float rhs) const {
	__m128 f(_mm_load1_ps(&rhs));
	return Matrix_16f(	_mm_div_ps(v[0], f),
						_mm_div_ps(v[1], f),
						_mm_div_ps(v[2], f),
						_mm_div_ps(v[3], f));
}

/**************************************************************************************************
* Divides the matrix by a vector of scalars
**************************************************************************************************/
inline Matrix_16f Matrix_16f::operator/(const __m128& rhs) const {
	return Matrix_16f(	_mm_div_ps(v[0], rhs),
						_mm_div_ps(v[1], rhs),
						_mm_div_ps(v[2], rhs),
						_mm_div_ps(v[3], rhs));
}

/**************************************************************************************************
* Multiplies the matrix by a scalar
**************************************************************************************************/
inline Matrix_16f operator*(float lhs, const Matrix_16f& rhs) {
	__m128 f(_mm_load1_ps(&lhs));
	return Matrix_16f(	_mm_mul_ps(f, rhs.v[0]),
						_mm_mul_ps(f, rhs.v[1]),
						_mm_mul_ps(f, rhs.v[2]),
						_mm_mul_ps(f, rhs.v[3]));
}

/**************************************************************************************************
* Multiplies the matrix by a vector of scalars
**************************************************************************************************/
inline Matrix_16f operator*(const __m128& lhs, const Matrix_16f& rhs) {
	return Matrix_16f(	_mm_mul_ps(lhs, rhs.v[0]),
						_mm_mul_ps(lhs, rhs.v[1]),
						_mm_mul_ps(lhs, rhs.v[2]),
						_mm_mul_ps(lhs, rhs.v[3]));
}

/**************************************************************************************************
* Equivalence
**************************************************************************************************/
inline bool Matrix_16f::operator==(const Matrix_16f& rhs) const {
	return (_mm_movemask_ps(_mm_cmpeq_ps(v[0], rhs.v[0])) == 0x0F &&
			_mm_movemask_ps(_mm_cmpeq_ps(v[1], rhs.v[1])) == 0x0F &&
			_mm_movemask_ps(_mm_cmpeq_ps(v[2], rhs.v[2])) == 0x0F &&
			_mm_movemask_ps(_mm_cmpeq_ps(v[3], rhs.v[3])) == 0x0F);
}

/**************************************************************************************************
* Non-equivalence
**************************************************************************************************/
inline bool Matrix_16f::operator!=(const Matrix_16f& rhs) const {
	return (_mm_movemask_ps(_mm_cmpeq_ps(v[0], rhs.v[0])) != 0x0F ||
			_mm_movemask_ps(_mm_cmpeq_ps(v[1], rhs.v[1])) != 0x0F ||
			_mm_movemask_ps(_mm_cmpeq_ps(v[2], rhs.v[2])) != 0x0F ||
			_mm_movemask_ps(_mm_cmpeq_ps(v[3], rhs.v[3])) != 0x0F);
}

/**************************************************************************************************
* Load the identity matrix
**************************************************************************************************/
inline Matrix_16f* MatrixIdentity_16f(Matrix_16f* pOut) {
	pOut->v[0] = _mm_set_ps(0.0f, 0.0f, 0.0f, 1.0f);
	pOut->v[1] = _mm_set_ps(0.0f, 0.0f, 1.0f, 0.0f);
	pOut->v[2] = _mm_set_ps(0.0f, 1.0f, 0.0f, 0.0f);
	pOut->v[3] = _mm_set_ps(1.0f, 0.0f, 0.0f, 0.0f);
	return pOut;
}

/**************************************************************************************************
* Tests against matrix identity
**************************************************************************************************/
inline bool MatrixIsIdentity_16f(const Matrix_16f* pM) {
	return (_mm_movemask_ps(_mm_cmpeq_ps(pM->v[0], _mm_set_ps(0.0f, 0.0f, 0.0f, 1.0f))) == 0x0F &&
			_mm_movemask_ps(_mm_cmpeq_ps(pM->v[1], _mm_set_ps(0.0f, 0.0f, 1.0f, 0.0f))) == 0x0F &&
			_mm_movemask_ps(_mm_cmpeq_ps(pM->v[2], _mm_set_ps(0.0f, 1.0f, 0.0f, 0.0f))) == 0x0F &&
			_mm_movemask_ps(_mm_cmpeq_ps(pM->v[3], _mm_set_ps(1.0f, 0.0f, 0.0f, 0.0f))) == 0x0F);
}

/**************************************************************************************************
* Sum of two matrices
**************************************************************************************************/
inline Matrix_16f* MatrixAdd_16f(Matrix_16f* pOut, const Matrix_16f* pM1, const Matrix_16f* pM2) {
	pOut->v[0] = _mm_add_ps(pM1->v[0], pM2->v[0]);
	pOut->v[1] = _mm_add_ps(pM1->v[1], pM2->v[1]);
	pOut->v[2] = _mm_add_ps(pM1->v[2], pM2->v[2]);
	pOut->v[3] = _mm_add_ps(pM1->v[3], pM2->v[3]);
	return pOut;
}

/**************************************************************************************************
* Difference between two matrices
**************************************************************************************************/
inline Matrix_16f* MatrixSub_16f(Matrix_16f* pOut, const Matrix_16f* pM1, const Matrix_16f* pM2) {
	pOut->v[0] = _mm_sub_ps(pM1->v[0], pM2->v[0]);
	pOut->v[1] = _mm_sub_ps(pM1->v[1], pM2->v[1]);
	pOut->v[2] = _mm_sub_ps(pM1->v[2], pM2->v[2]);
	pOut->v[3] = _mm_sub_ps(pM1->v[3], pM2->v[3]);
	return pOut;
}